#include "buaa/utils.h"


std::vector<Eigen::Vector3i> getbresenham3D(Eigen::Vector3i start_idx, Eigen::Vector3i end_index)
{  

	std::vector<Eigen::Vector3i> bresenham3Dpath;

    int dx = abs(end_index(0) - start_idx(0) );
    int dy =abs(end_index(1) - start_idx(1) );
    int dz =abs(end_index(2) - start_idx(2) );
    int xs,ys,zs;
    xs=(end_index(0)> start_idx(0))?1:-1;
    ys=(end_index(1)> start_idx(1))?1:-1;
    zs=(end_index(2)> start_idx(2))?1:-1;

    int x1,y1,z1,x2,y2,z2;
    x1=start_idx(0);    y1=start_idx(1);    z1=start_idx(2);
    x2=end_index(0);    y2=end_index(1);    z2=end_index(2);

    int p1,p2;
    // # Driving axis is X-axis"
    if (dx >= dy && dx >= dz)
    {
        p1 = 2 * dy - dx;
        p2 = 2 * dz - dx;
        while (x1 != x2)
        {
            x1 += xs;
            if (p1 >= 0)
            {
                y1 += ys;
                p1 -= 2 * dx;
            }

            if (p2 >= 0)
            {
                z1 += zs;
                p2 -= 2 * dx;
            }

            p1 += 2 * dy;
            p2 += 2 * dz;
            Eigen::Vector3i tmp(x1,y1,z1);
            bresenham3Dpath.push_back(tmp);
        }

    }

    // # Driving axis is Y-axis"
    else if (dy >= dx and dy >= dz)
    {
        p1 = 2 * dx - dy;
        p2 = 2 * dz - dy;
        while (y1 != y2)
        {
            y1 += ys;
            if (p1 >= 0)
            {
                x1 += xs;
                p1 -= 2 * dy;
            }
            if (p2 >= 0)
            {
                z1 += zs;
                p2 -= 2 * dy;
            }
            p1 += 2 * dx;
            p2 += 2 * dz;
            Eigen::Vector3i tmp(x1,y1,z1);
            bresenham3Dpath.push_back(tmp);        
        }

    }

    // # Driving axis is Z-axis"
    else{
        p1 = 2 * dy - dz;
        p2 = 2 * dx - dz;
        while (z1 != z2)
        {
            z1 += zs;
            if (p1 >= 0)
            {
                y1 += ys;
                p1 -= 2 * dz;
            }
            if (p2 >= 0)
            {
                x1 += xs;
                p2 -= 2 * dz;
            }
            p1 += 2 * dy;
            p2 += 2 * dx;
            Eigen::Vector3i tmp(x1,y1,z1);
            bresenham3Dpath.push_back(tmp);        
        }
    }

    return bresenham3Dpath;

}



// int main()
// {
//     typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

//     polygon green, blue;

//  	polygon polygon1{{{0.0, 0.0}, {0.0,1.0}, {1.0,1.0}, {1.0,0.0}, {0.0, 0.0}}};
//  	polygon polygon2{{{0.5, 0.0}, {0.5,1.0}, {1.5,1}, {1.5,0}, {0.5, 0.0}}};

//     double area = bg::area(polygon1);
//     std::cout << "Area: " << area << std::endl;

//     double area2 = bg::area(polygon2);
//     std::cout << "Area2: " << area2 << std::endl;

//     boost::geometry::read_wkt(
//         "POLYGON((0 0,1 0,1 1,0 1,0 0))", green);

//     boost::geometry::read_wkt(
//         "POLYGON((0.5 0,1.5 0,1.5 1,0.5 1,0.5 0))", blue);

//     std::deque<polygon> output;
//     boost::geometry::intersection(polygon1, polygon2, output);

//     int i = 0;
//     std::cout << "green && blue:" << std::endl;
//     BOOST_FOREACH(polygon const& p, output)
//     {
//         std::cout << i++ << ": " << boost::geometry::area(p) << std::endl;
//     }


//     return 0;
// }